#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <INA219_WE.h>  // INA219库

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define POT_PIN       34    // 电位器连接引脚
#define THROTTLE_PIN  15    // 油门开关连接引脚
#define DIRECTION_PIN 13    // 方向开关连接引脚

// INA219配置
#define I2C_ADDRESS 0x40
#define INA219_SDA 19
#define INA219_SCL 23

// OLED配置
#define OLED_SDA 4
#define OLED_SCL 5

// 电机引脚定义
#define LEFT_RPWM  14    // 左电机前进
#define LEFT_LPWM  27    // 左电机后退
#define RIGHT_RPWM 26    // 右电机前进
#define RIGHT_LPWM 12    // 右电机后退

// PWM参数
#define PWM_FREQ 5000    // PWM频率
#define PWM_RES  8       // PWM分辨率(8位=0-255)
#define MAX_FORWARD_RPM 200  // 最大正转速度
#define MAX_REVERSE_RPM 120   // 最大反转速度
// 死区阈值
#define DEADZONE_THRESHOLD 0.1

// 电池参数 (2并3串 21700锂电池)
#define BAT_FULL_VOLTAGE 12.6    // 满电电压 (3*4.2V)
#define BAT_EMPTY_VOLTAGE 10.5   // 空电电压 (3*3.5V)
#define BAT_WARNING_VOLTAGE 11.1 // 低电量警告电压 (3*3.7V)

// 创建第二个I2C实例
TwoWire I2C_OLED = TwoWire(0);  // 使用Wire0（默认I2C总线）
TwoWire I2C_INA219 = TwoWire(1); // 使用Wire1（第二个I2C总线）

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2C_OLED, OLED_RESET);
INA219_WE ina219(&I2C_INA219, I2C_ADDRESS);  // INA219实例使用第二个I2C总线

// INA219状态变量
bool ina219Connected = false;
bool ina219Overflow = false;
float busVoltage = 0.0;
float current_mA = 0.0;

void setup() {
  Serial.begin(115200);
  
  // 初始化引脚
  pinMode(THROTTLE_PIN, INPUT_PULLUP);  // 油门开关
  pinMode(DIRECTION_PIN, INPUT_PULLUP); // 方向开关
  
  // 初始化电机控制引脚
  ledcSetup(0, PWM_FREQ, PWM_RES);  // 左电机前进
  ledcAttachPin(LEFT_RPWM, 0);
  ledcSetup(1, PWM_FREQ, PWM_RES);  // 左电机后退
  ledcAttachPin(LEFT_LPWM, 1);
  ledcSetup(2, PWM_FREQ, PWM_RES);  // 右电机前进
  ledcAttachPin(RIGHT_RPWM, 2);
  ledcSetup(3, PWM_FREQ, PWM_RES);  // 右电机后退
  ledcAttachPin(RIGHT_LPWM, 3);
  
  // 初始化OLED使用的I2C0总线 (SDA=4, SCL=5)
  I2C_OLED.begin(OLED_SDA, OLED_SCL, 100000); // 100kHz速度
  
  // 初始化OLED显示
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while(1);
  }
  
  // 初始化INA219使用的I2C1总线 (SDA=19, SCL=23)
  I2C_INA219.begin(INA219_SDA, INA219_SCL, 100000); // 100kHz速度
  
  // 初始化INA219
  ina219Connected = ina219.init();
  
  if(ina219Connected) {
    ina219.setADCMode(SAMPLE_MODE_128);
    ina219.setMeasureMode(CONTINUOUS);
    ina219.setPGain(PG_320);
    ina219.setBusRange(BRNG_32);
    
    // 设置校正因子（根据实际需要调整）
    ina219.setCorrectionFactor(0.98);
  }
  
  // 显示初始信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.println("EV Control System");
  display.print("INA219: ");
  display.println(ina219Connected ? "OK" : "FAIL");
  display.display();
  delay(1000);
}

void loop() {
  // 读取INA219数据
  readINA219();
  
  // 读取油门状态（改进点1：使用更可靠的读取方式）
  bool throttlePressed = (digitalRead(THROTTLE_PIN) == LOW);  // 明确判断低电平
  
  // 读取方向状态
  bool directionForward = (digitalRead(DIRECTION_PIN) == HIGH); // HIGH=前进
  
  // 读取电位器值
  int potValue = analogRead(POT_PIN);
  float voltage = potValue * (3.3 / 4095.0);
  
  // 计算转速比例
  float speedRatio = constrain(voltage / 3.3, 0.0, 1.0);
  
  // 计算理论转速（无论油门是否按下）
  float theoreticalRPM = 0;
  if (speedRatio > DEADZONE_THRESHOLD) {
    if (directionForward) {
      theoreticalRPM = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, MAX_FORWARD_RPM);
    } else {
      theoreticalRPM = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, MAX_REVERSE_RPM);
    }
  }
  
  // 计算实际电机控制值
  float actualRPM = theoreticalRPM; // 改进点2：始终使用理论值
  String motorState = "STOP";
  String directionState = directionForward ? "FORWARD" : "REVERSE";
  
 
    if (throttlePressed) {
      if (speedRatio > DEADZONE_THRESHOLD) {
        motorState = "RUNNING";
        
        if (directionForward) {
          // 前进模式
          int pwmValue = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, 255);
          ledcWrite(0, pwmValue);  // 左前进
          ledcWrite(1, 0);         // 左后退
          ledcWrite(2, pwmValue);  // 右前进
          ledcWrite(3, 0);         // 右后退
        } else {
          // 后退模式
          int pwmValue = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, 90);
          ledcWrite(0, 0);         // 左前进
          ledcWrite(1, pwmValue);  // 左后退
          ledcWrite(2, 0);         // 右前进
          ledcWrite(3, pwmValue);  // 右后退
        }
      } else {
        stopMotors();
        motorState = "DEADZONE";
      }
    } else {
      stopMotors();
    }
  
  
  // 更新OLED显示（改进点3：始终显示实际转速）
  updateDisplay(throttlePressed, voltage, actualRPM, 
               motorState, 
               throttlePressed ? "PRESSED" : "RELEASED", 
               directionState);
  
  delay(50);
}

// 读取INA219数据
void readINA219() {
  if(!ina219Connected) return;
  
  busVoltage = ina219.getBusVoltage_V();
  current_mA = ina219.getCurrent_mA();
  ina219Overflow = ina219.getOverflow();
  
  // 调试输出
  Serial.print("电压[V]: "); Serial.print(busVoltage);
  Serial.print(" 电流[mA]: "); Serial.println(current_mA);
  if(ina219Overflow) Serial.println("INA219 Overflow!");
}

// 停止所有电机
void stopMotors() {
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
}

// 浮点数映射
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// 更新OLED显示
void updateDisplay(bool throttlePressed, float voltage, float rpm, 
                  String motorState, String throttleState, String directionState) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  
  // 第1行: 电压和电流显示
  display.setCursor(0, 0);
  display.print("V:");
  if(ina219Connected && !ina219Overflow) {
    display.print(busVoltage, 2);
    display.print("V");
  } else {
    display.print(ina219Connected ? "OVF!" : "N/A");
  }
  
  display.setCursor(64, 0);
  display.print("I:");
  if(ina219Connected && !ina219Overflow) {
    display.print(current_mA, 2);
    display.print("mA");
  } else {
    display.print(ina219Connected ? "OVF!" : "N/A");
  }
  
  // 第2行: 方向状态
  display.setCursor(0, 10);
  display.print("Dir:");
  display.print(directionState);
  
  // 第3行: 油门状态
  display.setCursor(0, 20);
  display.print("Thr:");
  display.print(throttleState);
  
  // 第4行: 电机状态
  display.setCursor(0, 30);
  display.print("Motor:");
  display.print(motorState);
  
  // 第5行: RPM显示（改进点：始终显示实际RPM）
  display.setCursor(80, 20);
  display.print("RPM:");
  display.print(rpm, 0);
  
  // 电池电量计算
  float batteryPercent = 0;
  if(ina219Connected && !ina219Overflow) {
    batteryPercent = calculateBatteryPercent(busVoltage);
  }
  
  // 第6行: 电池电量进度条
  drawBatteryBar(0, 40, 128, 8, batteryPercent/100.0);
  
  // 电池百分比标签
  display.setCursor(55, 40);
  if(ina219Connected && !ina219Overflow) {
    display.print((int)batteryPercent);
    display.print("%");
    
    // 低电量警告
    if(busVoltage < BAT_WARNING_VOLTAGE) {
      display.setCursor(0, 40);
      display.print("LOW BATTERY!");
    }
  } else {
    display.print("BAT:N/A");
  }
  
  // 第7行: 速度进度条
  float maxRpm = directionState == "FORWARD" ? MAX_FORWARD_RPM : MAX_REVERSE_RPM;
  float progress = (maxRpm > 0) ? constrain(rpm / maxRpm, 0.0, 1.0) : 0.0;
  drawProgressBar(10, 50, 108, 8, progress, directionState, throttlePressed);
  
  // 进度条标签
  display.setCursor(0, 50);
  display.print("0");
  display.setCursor(50, 50);
  display.print((int)(progress * 100));
  display.print("%");
  display.setCursor(110, 50);
  display.print(maxRpm, 0);
  
  display.display();
}

// 精确的电量计算函数
float calculateBatteryPercent(float voltage) {
  // 基于21700锂电池放电曲线建模
  if (voltage >= 12.6) return 100.0;     // 满电
  else if (voltage >= 12.3) return 80.0;  // 80%
  else if (voltage >= 12.0) return 60.0;  // 60%
  else if (voltage >= 11.7) return 40.0;  // 40%
  else if (voltage >= 11.4) return 20.0;  // 20%
  else if (voltage >= 11.1) return 5.0;   // 5%
  else return 0.0;                       // 空电
}

// 电池电量进度条
void drawBatteryBar(int x, int y, int width, int height, float percent) {
  // 绘制外框
  display.drawRect(x, y, width, height, WHITE);
  
  // 电池正极标识
  display.fillRect(x + width, y + 2, 2, height - 4, WHITE);
  
  // 计算填充宽度
  int fillWidth = (int)(percent * (width - 2));
  fillWidth = constrain(fillWidth, 0, width - 2);
  
  // 根据电量选择颜色和警告
  if(percent > 0.2) {
    display.fillRect(x + 1, y + 1, fillWidth, height - 2, WHITE);
  } else {
    // 低电量时闪烁效果 (每秒闪烁一次)
    static unsigned long lastBlink = 0;
    static bool blinkState = false;
    
    if(millis() - lastBlink > 500) {
      blinkState = !blinkState;
      lastBlink = millis();
    }
    
    if(blinkState) {
      display.fillRect(x + 1, y + 1, fillWidth, height - 2, WHITE);
    }
  }
}

// 速度进度条绘制
void drawProgressBar(int x, int y, int width, int height, float progress, String state, bool isActive) {
  display.drawRect(x, y, width, height, WHITE);
  int fillWidth = (int)(progress * (width - 2));
  fillWidth = constrain(fillWidth, 0, width - 2);
  
  // 根据是否激活决定填充颜色
  uint16_t fillColor = isActive ? WHITE : SSD1306_INVERSE;
  
  if (state == "REVERSE") {
    display.fillRect(x + width - fillWidth - 1, y + 1, fillWidth, height - 2, fillColor);
  } else {
    display.fillRect(x + 1, y + 1, fillWidth, height - 2, fillColor);
  }
}